#include "LKH.h"
#include "Heap.h"

#define Tail SucSaved

/*
 * The GreedyTour function computes a tour using either
 *
 *      (1) Nearest Neighbor (NEAREST-NEIGHBOR),
 *      (2) Bentley's multiple fragment heuristic (GREEDY),
 *      (3) Boruvka (BORUVKA), or
 *      (4) Applegate, Cook and Rohe's Quick-Boruvka heuristic
 *          (QUICK-BORUVKA).
 *
 *    J. L. Bentley,
 *    Fast Algorithms for Geometric Traveling Salesman Problems,
 *    ORSA Journal on Computing, Vol. 4, 4, pp. 387-411 (1992).
 *
 *    D. Applegate, W. Cook, and A. Rohe,
 *    Chained Lin-Kernighan for large traveling salesman problems.
 *    Technical Report No. 99887, Forschungsinstitut
 *    fuer Diskrete Mathematik, University of Bonn, Germany (1999).
 *
 * The function returns the cost of the resulting tour.
 *
 * Nearest neighbor searching is performed by using the candidate graph.
 * To provide different tours a randomized nearest neighbor search is used.
 * The first found feasible neighbor is returned with probability 2/3.
 *
 * A heap contains the nearest neighbor edges when the multiple fragment
 * heuristic is used.
 */

#define Degree V     /* Number of edges currently adjacent to a node */
#define Mark LastV   /* Mark of a node during breadth-first search   */
#define Level BestPi /* Search level */

static Node* NearestNeighbor(Node* From);
static Node* NearestInList(Node* From, Node* First);
static int MayBeAddedToFragments(Node* From, Node* To);
static void AddEdgeToFragments(Node* From, Node* To);
static void RemoveFromList(Node* N, Node** First);
static int compareX(const void* Na, const void* Nb);
static int compareCost(const void* Na, const void* Nb);

static int EdgesInFragments;
static GainType Cost;

GainType GreedyTour() {
  Node *From, *To, *First, *Last = 0, **Perm;
  int Count, i;
  double EntryTime = GetTime();

  if (TraceLevel >= 1) {
    if (InitialTourAlgorithm == BORUVKA)
      printff("Boruvka = ");
    else if (InitialTourAlgorithm == GREEDY)
      printff("Greedy = ");
    else if (InitialTourAlgorithm == NEAREST_NEIGHBOR)
      printff("Nearest-Neighbor = ");
    else if (InitialTourAlgorithm == QUICK_BORUVKA)
      printff("Quick-Boruvka = ");
  }
  Cost = 0;
  EdgesInFragments = 0;
  From = FirstNode;
  do {
    From->Degree = 0;
    From->Tail = From;
    From->Mark = 0;
    From->Next = From->Suc;
    From->Pred = From->Suc = 0;
  } while ((From = From->Next) != FirstNode);
  Count = 0;
  for (;;) {
    if ((To = NearestNeighbor(From)) && FixedOrCommon(From, To))
      AddEdgeToFragments(From, To);
    else {
      if ((From->Nearest = To)) {
        if (InitialTourAlgorithm == GREEDY) {
          From->Rank = From->Cost;
          HeapLazyInsert(From);
        } else
          Count++;
      }
      if ((From = From->Next) == FirstNode) break;
    }
  }
  if (InitialTourAlgorithm == NEAREST_NEIGHBOR) {
    if (EdgesInFragments < Dimension) {
      while (From->Degree == 2) From = From->Tail;
      for (;;) {
        int Min = INT_MAX, d;
        Node* Nearest = 0;
        while ((To = NearestNeighbor(From))) {
          AddEdgeToFragments(From, To);
          if (EdgesInFragments == Dimension) break;
          From = To->Degree < 2 ? To : To->Tail;
          while (From->Degree == 2) From = From->Tail;
        }
        if (EdgesInFragments == Dimension) break;
        To = FirstNode;
        do {
          if (MayBeAddedToFragments(From, To) && (!c || c(From, To) < Min) &&
              (d = C(From, To)) < Min) {
            Min = From->Cost = d;
            Nearest = To;
          }
        } while ((To = To->Next) != FirstNode);
        assert(Nearest);
        To = Nearest;
        AddEdgeToFragments(From, To);
        if (EdgesInFragments == Dimension) break;
        while (From->Degree == 2) From = From->Tail;
        From = To->Degree < 2 ? To : To->Tail;
      }
    }
  } else {
    if (InitialTourAlgorithm == GREEDY) {
      Heapify();
      while ((From = HeapDeleteMin())) {
        To = From->Nearest;
        if (MayBeAddedToFragments(From, To)) AddEdgeToFragments(From, To);
        if ((From->Nearest = NearestNeighbor(From))) {
          From->Rank = From->Cost;
          HeapInsert(From);
        }
      }
    } else {
      Perm = (Node**)malloc(Count * sizeof(Node*));
      for (From = FirstNode, i = 0; i < Count; From = From->Next)
        if (From->Nearest) Perm[i++] = From;
      if (InitialTourAlgorithm == QUICK_BORUVKA) {
        qsort(Perm, Count, sizeof(Node*), compareX);
        for (i = 0; i < Count; i++) {
          From = Perm[i];
          if ((To = NearestNeighbor(From))) {
            AddEdgeToFragments(From, To);
            i--;
          }
        }
      } else if (InitialTourAlgorithm == BORUVKA) {
        while (Count > 0) {
          qsort(Perm, Count, sizeof(Node*), compareCost);
          for (i = 0; i < Count; i++) {
            From = Perm[i];
            To = From->Nearest;
            if (MayBeAddedToFragments(From, To)) AddEdgeToFragments(From, To);
          }
          for (i = 0; i < Count;) {
            From = Perm[i];
            if (!(To = NearestNeighbor(From)))
              Perm[i] = Perm[--Count];
            else {
              From->Nearest = To;
              i++;
            }
          }
        }
      }
      free(Perm);
    }
    if (EdgesInFragments < Dimension) {
      /* Create a list of the degree-0 and degree-1 nodes */
      First = 0;
      From = FirstNode;
      do {
        if (From->Degree != 2) {
          From->OldSuc = First;
          if (First)
            First->OldPred = From;
          else
            Last = From;
          First = From;
        }
      } while ((From = From->Next) != FirstNode);
      First->OldPred = Last;
      Last->OldSuc = First;
      From = First;
      do {
        if ((From->Nearest = NearestInList(From, First))) {
          From->Rank = From->Cost;
          HeapLazyInsert(From);
        }
      } while ((From = From->OldSuc) != First);
      Heapify();
      /* Find the remaining fragments */
      while ((From = HeapDeleteMin())) {
        To = From->Nearest;
        if (MayBeAddedToFragments(From, To)) {
          AddEdgeToFragments(From, To);
          if (From->Degree == 2) RemoveFromList(From, &First);
          if (To->Degree == 2) RemoveFromList(To, &First);
        }
        if (From->Degree != 2 && (From->Nearest = NearestInList(From, First))) {
          From->Rank = From->Cost;
          HeapInsert(From);
        }
      }
      assert(EdgesInFragments == Dimension);
    }
  }
  /* Orient Pred and Suc so that the list of nodes represents a tour */
  To = FirstNode;
  From = To->Pred;
  do {
    if (To->Suc == From) {
      To->Suc = To->Pred;
      To->Pred = From;
    }
    From = To;
  } while ((To = From->Suc) != FirstNode);
  To->Pred = From;
  Cost /= Precision;
  CurrentPenalty = PLUS_INFINITY;
  CurrentPenalty = Penalty ? Penalty() : 0;
  if (TraceLevel >= 1) {
    if (Salesmen > 1 || ProblemType == SOP)
      printff(GainFormat "_" GainFormat, CurrentPenalty, Cost);
    else
      printff(GainFormat, Cost);
    if (Optimum != MINUS_INFINITY && Optimum != 0) {
      if (MTSPObjective == MINMAX || MTSPObjective == MINMAX_SIZE)
        printff(", Gap = %0.1f%%", 100.0 * (CurrentPenalty - Optimum) / Optimum);
      else
        printff(", Gap = %0.1f%%", 100.0 * (Cost - Optimum) / Optimum);
    }
    printff(", Time = %0.2f sec.\n", fabs(GetTime() - EntryTime));
  }
  return Cost;
}

/*
 * The NearestNeighbor function returns for a given node, From, the "nearest"
 * neighbor, To, for which the addition of the edge (From, To) will not make
 * it impossible to complete a tour.
 *
 * The neighbor is determined by searching the candidate graph breadth-first,
 * starting at the node From. If possible, the nearest node on level 1 is
 * chosen. Otherwise, on level 2. And so on. A queue is used for this search.
 *
 * Note that this algorithm finds an approximation to the nearest neighbor.
 * However, by giving candidate edges precedence over non-candidate edges the
 * algorithm will often produce better results than an algorithm that
 * determines the true nearest neighbors (for example, by using a KD-tree).
 */

static Node* NearestNeighbor(Node* From) {
  static int mark = 0;
  Candidate* NN;
  Node *To, *N, *First = 0, *Last = 0, *Nearest = 0;
  int MaxLevel = Dimension, Min = INT_MAX, d;

  if (From->Degree == 2) return 0;
  for (NN = From->CandidateSet; (To = NN->To); NN++) {
    if (FixedOrCommon(From, To) && MayBeAddedToFragments(From, To)) {
      From->Cost = NN->Cost;
      return To;
    }
  }
  From->Level = 0;
  if (++mark == 0) mark = 1;
  From->Mark = mark;
  /* Insert From into an empty queue */
  First = Last = From;
  From->OldSuc = 0;

  while ((N = First) && N->Level < MaxLevel) {
    /* Remove the first node from the queue */
    if (N == Last)
      First = Last = 0;
    else
      First = N->OldSuc;
    for (NN = N->CandidateSet; (To = NN->To); NN++) {
      if (To->Mark != mark) {
        To->Mark = mark;
        To->Level = N->Level + 1;
        if (MayBeAddedToFragments(From, To) &&
            (N == From ? (d = NN->Cost) < Min :
                         (!c || c(From, To) < Min) && (d = C(From, To)) < Min)) {
          Min = From->Cost = d;
          /* Randomization */
          if (!Nearest && Random() % 3 != 0) return To;
          Nearest = To;
          MaxLevel = To->Level;
        } else if (To->Level < MaxLevel) {
          /* Insert To as the last element of the queue */
          if (Last)
            Last->OldSuc = To;
          else
            First = To;
          Last = To;
          Last->OldSuc = 0;
        }
      }
    }
  }
  return Nearest;
}

/*
 * The NearestInList function returns for a given node, From, the "nearest"
 * neighbor, To, for which the addition of the edge (From, To) will not make
 * it impossible to complete a tour.
 *
 * The neighbor is determined by searching the list of nodes that are not in
 * any fragment.
 */

static Node* NearestInList(Node* From, Node* First) {
  Node *To, *Nearest = 0;
  int Min = INT_MAX, d;

  To = First;
  do {
    if (MayBeAddedToFragments(From, To) && (!c || c(From, To) < Min) && (d = C(From, To)) < Min) {
      Min = From->Cost = d;
      Nearest = To;
    }
  } while ((To = To->OldSuc) != First);
  return Nearest;
}

/*
 * The MayBeAddedToFragments is used to test if the addition of a given edge,
 * (From, To), makes it impossible to complete a tur.
 * If the edge may be added, the function returns 1; otherwise 0.
 */

static int MayBeAddedToFragments(Node* From, Node* To) {
  return From != To && From->Degree != 2 && To->Degree != 2 &&
         (From->Tail != To || EdgesInFragments == Dimension - 1) && !Forbidden(From, To);
}

/*
 * The AddEdgeToFragments function adds a given edge, (From, To), to the
 * current set of fragments.
 */

static void AddEdgeToFragments(Node* From, Node* To) {
  Node* Temp;

  if (!From->Pred)
    From->Pred = To;
  else
    From->Suc = To;
  if (!To->Pred)
    To->Pred = From;
  else
    To->Suc = From;
  From->Degree++;
  To->Degree++;
  Temp = From->Tail;
  Temp->Tail = To->Tail;
  To->Tail->Tail = Temp;
  EdgesInFragments++;
  Cost += From->Cost - From->Pi - To->Pi;
}

/*
 * The RemoveFromList function removes a given node, N, from the list of
 * non-fragment nodes. The parameter FIrst is a reference to the first node
 * of this list.
 */

static void RemoveFromList(Node* N, Node** First) {
  if (*First == N) *First = N->OldSuc;
  N->OldPred->OldSuc = N->OldSuc;
  N->OldSuc->OldPred = N->OldPred;
}

static int compareX(const void* Na, const void* Nb) {
  double x1 = (*(Node**)Na)->X;
  double y1 = (*(Node**)Na)->Y;
  double x2 = (*(Node**)Nb)->X;
  double y2 = (*(Node**)Nb)->Y;
  return x1 < x2 ? -1 : x1 > x2 ? 1 : y1 < y2 ? -1 : y1 > y2 ? 1 : 0;
}

static int compareCost(const void* Na, const void* Nb) {
  return (*(Node**)Na)->Cost - (*(Node**)Nb)->Cost;
}
